#include "tcp_bridge.h"
#include <boost/bind/bind.hpp>
using namespace boost::placeholders; 

#define SERVER_PORT 9091

#define TOPIC_AGV_MODE "/agvmode"                                  // 话题名称：控制方式
#define TOPIC_AUTO_STATUS "/auto_run_status"                       // 话题名称：运行状态
#define TOPIC_BAT_INFO "/batteryInfo"                              // 话题名称：电池状态
#define TOPIC_NAVI_TYPE "/navigation_type"                         // 话题名称：导航方式
#define TOPIC_EVENT "/eventstatus"                                 // 话题名称：异常事件
#define TOPIC_TAPE "/base/tape_sensor"                             // 话题名称：tape传感器
#define TOPIC_RFID "/base/rfid_sensor"                             // 话题名称：rfid传感器
#define TOPIC_INPUT "/readInputNew"                                // 话题名称：输入口状态
#define TOPIC_LANDMARK_U "/landmark_shelf"                         // 话题名称：二维码上读头数据
#define TOPIC_LANDMARK_D "/landmark"                               // 话题名称：二维码下读头数据
#define TOPIC_ODOM "/odom"                                         // 话题名称：里程计
#define TOPIC_CMD_VEL "/cmd_vel"                                   // 话题名称：速度下发指令
#define TOPIC_CALIB_PER_LIDAR "/lidarCalib_percent"                // 话题名称：激光雷达标定进度
#define TOPIC_CALIB_PER_QR "/qrCalib_percent"                      // 话题名称：二维码读头标定进度
#define TOPIC_CALIB_PER_ODOM "/agv_calib/odom_calib"               // 话题名称：里程计标定控制运动进度
#define TOPIC_REF_LIST_MAP "/g_reflector_map"                      // 话题名称：已生成的反光柱列表
#define TOPIC_REF_LIST_CUR "/reflector_match_lists"                // 话题名称：当前检测到的反光柱列表
#define TOPIC_REF_STATE "/reflector_state"                         // 话题名称：反光导航状态
#define TOPIC_JOINT_STATE "/joint_states"                          // 话题名称：电机状态
#define TOPIC_DRIVER_INFO "/driversInfo"                           // 话题名称：驱动状态
#define TOPIC_CANOPEN_NODE "/canopen_node_status"                  // 话题名称：CANOPEN节点状态
#define TOPIC_SAVE_MAP_PGS "/save_map_progress"                    // 话题名称：保存地图进度
#define TOPIC_EDIT_MAP_PGS "/edit_map_progress"                    // 话题名称：编辑地图进度
#define TOPIC_SCAN_MATCHED "/scan_matched_points"                  // 话题名称：世界坐标系下点云
#define TOPIC_ROBOT_STATUS "/robot_status"                         // 话题名称：AGV导航状态和位姿信息
#define TOPIC_AGV_POSE "/pose"                                     // 话题名称：AGV位姿信息（可获取置信度等）
#define TOPIC_MAP_DATA "/map_data"                                 // 话题名称：SLAM地图数据
#define TOPIC_SCAN "/scan"                                         // 话题名称：激光雷达坐标系下的原始点云数据（总的）
#define TOPIC_SCAN_1 "/scan_1"                                     // 话题名称：激光雷达1坐标系下的原始点云数据
#define TOPIC_SCAN_2 "/scan_2"                                     // 话题名称：激光雷达2坐标系下的原始点云数据
#define TOPIC_SCAN_FLT_1 "/scan_flt_1"                             // 话题名称：激光雷达1坐标系下的滤波点云数据
#define TOPIC_SCAN_FLT_2 "/scan_flt_2"                             // 话题名称：激光雷达2坐标系下的滤波点云数据
#define TOPIC_CALIB_PER_2LIDAR "/CalibProgress"                    // 话题名称：双激光雷达标定进度
#define TOPIC_REF_LANDMARK "/agv_reflector_landmark"               // 话题名称：反光物辅助导航时，识别到的柱子
#define TOPIC_OBS_LEVEL "/agv_obstacle_level"                      // 话题名称：避障等级

CGetAgvStates::CGetAgvStates(std::shared_ptr<rclcpp::Node> nh)
{
    m_nh = nh;
    init();
}

CGetAgvStates::~CGetAgvStates()
{
    if (m_srvOperator)
    {
        delete m_srvOperator;
        m_srvOperator = nullptr;
    }
}
void CGetAgvStates::setSendDataProcessor(std::function<void(std::vector<int>, std::string)> processor)
{
    m_vSendDataProcessor = processor;
    if (m_srvOperator)
    {
        m_srvOperator->setSendDataProcessor(processor);
    }
}

void CGetAgvStates::recvData(const int id, std::string &buffer)
{
    // 删除空格、换行符
    buffer.erase(std::remove_if(buffer.begin(), buffer.end(), [](unsigned char c)
                                { return c == '\n'; }),
                 buffer.end());
    buffer.erase(std::remove_if(buffer.begin(), buffer.end(), [](unsigned char c)
                                { return c == ' '; }),
                 buffer.end());

    m_recvMap[id] += buffer;
    // RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), "client:"
    //                 << id << ",recv data: \n"
    //                 << buffer << "\n"
    //                 << "buffer is:\n"
    //                 << m_recvMap.at(id) << "\n");

    // 搜索完整的json包
    while (true)
    {
        // 每次开始前，先清理一次无效数据
        if (clearInvalidData(m_recvMap[id]))
        {
            RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), "Remaining data:\n"
                            << m_recvMap.at(id) << "\n");
        }

        // 搜索两包分割特征标志符"}{"
        size_t idx = m_recvMap.at(id).find("}{", 0);
        if (idx == std::string::npos)
        {
            break; // 找不到退出
        }

        // 直接截
        std::string str_sub = m_recvMap.at(id).substr(0, idx + 1); // 截取前边的，到“}”

        m_recvMap[id] = m_recvMap.at(id).substr(idx + 1); // 删除前边的，保留“{”之后的

        // 转为json,成功则顺便处理
        convertJsonValue(id, str_sub);
    }
 
    // 是不是单独完整一帧
    if (convertJsonValue(id, m_recvMap.at(id)))
    {
        m_recvMap[id].clear();
    }
}
void CGetAgvStates::clientConnStateChanged(const int id, const bool state)
{
    if (!state)
    {
        // 删除所有订阅中的该id，若idlist为空，则删除该订阅
        for (auto it = m_sendData.begin(); it != m_sendData.end();)
        {
            // Remove all occurrences of 6 from the vector
            it->second.clientIdRemove(id);

            // Check if the vector is empty after removal
            if (it->second.getClientIdList().empty())
            {
                it = m_sendData.erase(it); // If the vector is empty, erase the element from the map
            }
            else
            {
                ++it; // Move to the next element if the vector is not empty
            }
        }

        // 删除接收数组中的该id
        m_recvMap.erase(id); // 接收数组
    }
}

// #1 初始化
bool CGetAgvStates::init()
{
    RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "Class GetAgvStates init start !");
    if (m_cGetAgvStatesInitOk)
    {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Class GetAgvStates is already init");
        return false;
    }

    m_srvOperator = new CServiceOperate(m_nh);
    m_subscribers.resize(35);
    subscriberInit<agv_msgs::msg::AGVMode>(0, TOPIC_AGV_MODE);
    subscriberInit<agv_msgs::msg::AgvAutoStatus>(1, TOPIC_AUTO_STATUS);
    subscriberInit<agv_msgs::msg::BatInfo>(2, TOPIC_BAT_INFO);
    subscriberInit<agv_msgs::msg::NavType>(3, TOPIC_NAVI_TYPE);
    subscriberInit<agv_msgs::msg::AGVEventStatus>(4, TOPIC_EVENT);
    // subscriberInit<agv_msgs::msg::tape_sensors>(5, TOPIC_TAPE);
    // subscriberInit<agv_msgs::msg::rfid_sensor>(6, TOPIC_RFID);
    subscriberInit<agv_msgs::msg::ReadInPutsNew>(7, TOPIC_INPUT);
    subscriberInit<geometry_msgs::msg::PoseStamped>(8, TOPIC_LANDMARK_U);
    subscriberInit<geometry_msgs::msg::PoseStamped>(9, TOPIC_LANDMARK_D);
    subscriberInit<agv_msgs::msg::OdometryLite>(10, TOPIC_ODOM);
    subscriberInit<geometry_msgs::msg::TwistStamped>(11, TOPIC_CMD_VEL);
    subscriberInit<agv_msgs::msg::LidarCalibPercent>(12, TOPIC_CALIB_PER_LIDAR);
    subscriberInit<agv_msgs::msg::QrCalibPercent>(13, TOPIC_CALIB_PER_QR);
    subscriberInit<agv_msgs::msg::AgvOdomCalib>(14, TOPIC_CALIB_PER_ODOM);
    // subscriberInit<reflector_slam_ros::msg::ReflectorListPose>(15, TOPIC_REF_LIST_MAP);
    // subscriberInit<reflector_slam_ros::msg::ReflectorListPose>(16, TOPIC_REF_LIST_CUR);
    // subscriberInit<reflector_slam_ros::msg::ReflectorStatus>(17, TOPIC_REF_STATE);
    subscriberInit<sensor_msgs::msg::JointState>(18, TOPIC_JOINT_STATE);
    subscriberInit<agv_msgs::msg::DriversInfo>(19, TOPIC_DRIVER_INFO);
    subscriberInit<agv_msgs::msg::CANopenNodeStatus>(20, TOPIC_CANOPEN_NODE);
    subscriberInit<slam_ros_msgs::msg::MapdataProgress>(21, TOPIC_SAVE_MAP_PGS);
    subscriberInit<slam_ros_msgs::msg::MapdataProgress>(22, TOPIC_EDIT_MAP_PGS);
    subscriberInit<sensor_msgs::msg::PointCloud>(23, TOPIC_SCAN_MATCHED);
    subscriberInit<slam_ros_msgs::msg::RobotStatus>(24, TOPIC_ROBOT_STATUS);
    subscriberInit<agv_msgs::msg::PoseWithConfidence>(25, TOPIC_AGV_POSE);
    subscriberInit<agv_msgs::msg::MapPng>(26, TOPIC_MAP_DATA);
    subscriberInit<sensor_msgs::msg::LaserScan>(27, TOPIC_SCAN);
    subscriberInit<sensor_msgs::msg::LaserScan>(28, TOPIC_SCAN_1);
    subscriberInit<sensor_msgs::msg::LaserScan>(29, TOPIC_SCAN_2);
    subscriberInit<sensor_msgs::msg::LaserScan>(30, TOPIC_SCAN_FLT_1);
    subscriberInit<sensor_msgs::msg::LaserScan>(31, TOPIC_SCAN_FLT_2);
    subscriberInit<agv_msgs::msg::CalibProgress>(32, TOPIC_CALIB_PER_2LIDAR);
    subscriberInit<slam_ros_msgs::msg::LandmarkListPose>(33, TOPIC_REF_LANDMARK);
    subscriberInit<agv_msgs::msg::AvoidObstaInfo>(34, TOPIC_OBS_LEVEL);

    m_cGetAgvStatesInitOk = true;
    RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "Class GetAgvStates init finish !");
    return true;
}

template <typename T>
void CGetAgvStates::subscriberInit(const int index, const std::string &topic)
{
    RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), "create topic:" << topic);
    m_subscribers[index] = m_nh->create_subscription<T>(topic, 1, [this, topic](const typename T::ConstPtr &msg)
        { this->subCallback(msg, topic); });
}

bool CGetAgvStates::clearInvalidData(std::string &str)
{
    bool rst = false;
    // 清楚无效格式数据#1，{{{，仅保留最后一个
    while (true)
    {
        size_t index_1 = str.find("{{", 0);
        if (index_1 == std::string::npos)
        {
            break;
        }
        str.erase(index_1, 1);
        RCLCPP_WARN_STREAM(rclcpp::get_logger("AgvTcpBridge"), "clear invalid data:'{'\n");
        rst = true;
    }

    // 清楚无效格式数据#2，第一个{之前的数据
    size_t index_2 = str.find("{", 0);
    if (index_2 == std::string::npos)
    {
        // 找不到直接删除全部
        str.clear();
        RCLCPP_WARN_STREAM(rclcpp::get_logger("AgvTcpBridge"), "clear all data!(All Invalid)\n");
        rst = true;
    }
    else if (index_2 != 0)
    {
        // 不是首位，则删除前边的，保留“{”及之后的
        RCLCPP_WARN_STREAM(rclcpp::get_logger("AgvTcpBridge"), "clear invalid data:" << str.substr(0, index_2) << "\n");
        str = str.substr(index_2);
        rst = true;
    }

    return rst;
}

bool CGetAgvStates::convertJsonValue(const int id, const std::string &str)
{
    // 转换
    if (!IsJsonIllegal(str.c_str()))
    {
        RCLCPP_WARN_STREAM(rclcpp::get_logger("AgvTcpBridge"), "str->json转换失败,str=\n"
                        << str << "\n");
        return false;
    }
    Json::Reader reader;
    Json::Value root;
    reader.parse(str, root);
    // RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), "str->json转换成功: json is \n"
    //                 << root << "\n");
    // 处理json
    parseJsonValue(id, root);
    return true;
}
bool CGetAgvStates::IsJsonIllegal(const char *jsoncontent)
{
    std::stack<char> jsonstr;
    const char *p = jsoncontent;
    char startChar = jsoncontent[0];
    char endChar = '\0';
    bool isObject = false; // 防止 {}{}的判断
    bool isArray = false;  // 防止[][]的判断

    while (*p != '\0')
    {
        endChar = *p;
        switch (*p)
        {
        case '{':
            if (!isObject)
            {
                isObject = true;
            }
            else if (jsonstr.empty()) // 对象重复入栈
            {
                return false;
            }
            jsonstr.push('{');
            break;
        case '"':
            if (jsonstr.empty() || jsonstr.top() != '"')
            {
                jsonstr.push(*p);
            }
            else
            {
                jsonstr.pop();
            }
            break;
        case '[':
            if (!isArray)
            {
                isArray = true;
            }
            else if (jsonstr.empty())
            {
                return false;
            }
            jsonstr.push('[');
            break;
        case ']':
            if (jsonstr.empty() || jsonstr.top() != '[')
            {
                return false;
            }
            else
            {
                jsonstr.pop();
            }
            break;
        case '}':
            if (jsonstr.empty() || jsonstr.top() != '{')
            {
                return false;
            }
            else
            {
                jsonstr.pop();
            }
            break;
        case '\\': // 被转义的字符,跳过
            p++;
            break;
        default:;
        }
        p++;
    }

    if (jsonstr.empty())
    {
        // 确保是对象或者是数组,之外的都不算有效
        switch (startChar) // 确保首尾符号对应
        {
        case '{':
        {
            if (endChar = '}')
            {
                return true;
            }
            return false;
        }
        case '[':
        {
            if (endChar = ']')
            {
                return true;
            }
            return false;
        }
        default:
            return false;
        }

        return true;
    }
    else
    {
        return false;
    }
}

void CGetAgvStates::parseJsonValue(const int id, const Json::Value &root)
{
    if (!root.isMember("op"))
    {
        RCLCPP_WARN_STREAM(rclcpp::get_logger("AgvTcpBridge"), "json data not have “op”!\n");
        return;
    }

    std::string op = root["op"].asString();
    if (op == "subscribe")
    {
        if (!root.isMember("topic"))
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("AgvTcpBridge"), "subscribe,but data not have “topic”!\n");
            return;
        }

        // 如果没有这个话题，则添加
        std::string topic = root["topic"].asString();
        if (m_sendData.find(topic) == m_sendData.end())
        {
            m_sendData.insert(std::make_pair(topic, CSubscriberInfo()));
            RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), "client: " << id << ",添加订阅: " << topic << "\n");
        }

        // 添加订阅id
        m_sendData.at(topic).clientIdAdd(id);

        // 存在rate，则设置
        if (root.isMember("throttle_rate"))
        {
            int rate = root["throttle_rate"].asInt();
            m_sendData.at(topic).setSendRate(id, rate);
            RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), "client: " << id << ",设置“" << topic << "”订阅频率: " << rate << "\n");
        }
        // 设置sendtime为往前数大于rate的值
        int rate = m_sendData.at(topic).getSendRate(id);
        auto t_past = std::chrono::system_clock::now() - std::chrono::milliseconds(2 * rate);
        m_sendData.at(topic).setSendTime(id, t_past);
    }
    else if (op == "unsubscribe")
    {
        if (!root.isMember("topic"))
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("AgvTcpBridge"), "unsubscribe,but data not have “topic”!\n");
            return;
        }

        // 移除id
        std::string topic = root["topic"].asString();
        if (m_sendData.find(topic) == m_sendData.end())
        {
            return;
        }

        m_sendData.at(topic).clientIdRemove(id);
        RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), "client: " << id << ",取消订阅: " << topic << "\n");

        // 最后一个id，则移除该键对
        if (m_sendData.at(topic).getClientIdList().empty())
        {
            m_sendData.erase(topic);
            RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), topic << "：无订阅！" << "\n");
        }
    }
    else if (op == "call_service")
    {
        m_srvOperator->recvJsonData_call(id, root);
    }
    else if (op == "publish")
    {
        m_srvOperator->recvJsonData_pub(root);
    }
    else
    {
        // 其他操作等
    }
}
std::vector<int> CGetAgvStates::getSendIdlist(const std::string &topic)
{
    std::vector<int> ids;

    // map中无该话题，直接返回
    if (m_sendData.find(topic) == m_sendData.end())
    {
        return ids;
    }

    // 判断哪些id到时间了
    auto curTime = std::chrono::high_resolution_clock::now();
    for (auto &pair : m_sendData.at(topic).getClientMap())
    {
        auto sendTime = pair.second.sendTime;
        auto rate = pair.second.rate;

        std::chrono::duration<double, std::milli> duration = curTime - sendTime;
        if (duration.count() >= rate)
        {
            ids.push_back(pair.first);
            m_sendData.at(topic).setSendTime(pair.first, curTime);
        }
    }

    return ids;
}
void CGetAgvStates::sendJsonData(const std::string &topic, const Json::Value &json_msg, const std::vector<int> &ids)
{
    Json::Value json_data;
    json_data["op"] = "publish";
    json_data["topic"] = topic;
    json_data["msg"] = json_msg;
    json_data["result"] = true;

    std::string str_json_data = Json::FastWriter().write(json_data);
    if (!str_json_data.empty() && str_json_data.back() == '\n')
    {
        str_json_data.pop_back();
    }
    m_vSendDataProcessor(ids, str_json_data);
}

// 回调
template <typename T>
void CGetAgvStates::subCallback(const T &msg, const std::string &topic)
{
    std::vector<int> ids = getSendIdlist(topic);
    if (ids.empty())
    {
        return;
    }

    Json::Value json_msg = agv_msgs_to_json(*msg);
    sendJsonData(topic, json_msg, ids);
}

/*------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------*/

// 节点主函数
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto nh = rclcpp::Node::make_shared("agv_tcp_bridge");
    setlocale(LC_ALL, "");
    rclcpp::Rate loop_rate(10);

    CGetAgvStates agvStates(nh);
    boost::asio::io_service io_server;
    server tcp_server(io_server, SERVER_PORT);

    // 发送数据回调
    agvStates.setSendDataProcessor([&tcp_server](std::vector<int> ids, std::string buffer)
                                   { tcp_server.sendData(ids, buffer); });

    // 连接变化回调
    tcp_server.setConnChangedProcessor([&agvStates](int id, bool state)
                                       { agvStates.clientConnStateChanged(id, state); });
    // 接收数据回调
    tcp_server.setRecvDataProcessor([&agvStates](int id, std::string buffer)
                                    { agvStates.recvData(id, buffer); });

    std::thread io_thread(boost::bind(&boost::asio::io_service::run, &io_server));

    RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "agv_tcp_bridge set ok ");
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(nh);
    executor.spin();

    // 停止io_service
    io_server.stop();
    io_thread.join();

    RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "agv_tcp_bridge Spinning node shutdown...");
    loop_rate.sleep();
    return 0;
}